/*
 * cRKF23.cpp
 *
 *  Created on: Oct 12, 2010
 *      Author: murmeli
 */

#include "cRKF23.h"

RKF23::RKF23() {
	double ivy, ivdy;
	setName("Runge-Kutta-Fehlberg2(3)");
	//tx and fx are cleared and set from cInfo when active potential is set.
	//If, for some reason, the active potential is not set and method is initiated we
	//set tx and fx to 0 and 10 respectivly.
	/*ivy=0.1;
	ivdy=0;
	setStep(0.01);
	set_tx(0);
	set_fx(10);
	set_ty(ivy);
	set_fy(ivy);
	set_tdy(ivdy);
	set_fdy(ivdy);*/
	setMinerror(0.0);
	setMaxerror(5e-3);
	setMinstep(5e-8);
	setMaxstep(5.0);
}

RKF23::~RKF23() {
	// TODO Auto-generated destructor stub
}

void RKF23::methodStep(double& step, vector<double> &xx, vector<double> &yy, vector<double> &dy, cDY *DY, cPot *V) {
	double k1,k2,k3,l1,l2,l3,E_est;
	bool done=false;
	while(done==false) {
//Calculate single step
		k1 = dy.back();
		l1 = DY->DE(xx.back(), V->potential(xx.back()), yy.back(), dy.back());

		k2 = dy.back() + step * l1;
		l2 = DY->DE(xx.back() + step, V->potential(xx.back() + step), yy.back() + step * k1, dy.back() + step * l1);

		k3 = dy.back() + (step/4.0) * (l1 + l2);
		l3 = DY->DE(xx.back() + step/2.0, V->potential(xx.back() + step/2.0), yy.back() + (step/4.0)*(k1+k2),dy.back()+(step/4.0)*(l1+l2));

//Calculate the error estimate for the step
		E_est = fabs(step*(2*k3-k1-k2)/3.0);
		//cout << "Error = " << E_est <<  " step = " << step << " |E_est - E_max| = " << fabs(E_est - getMaxerror()) << endl;

//If the error estimate is less than specified maximum error and larger than specified minimum error we are done.
		if(E_est < getMaxerror() && E_est > getMinerror()) {
			done = true;
		}
//If the error estimate is smaller than specified minimum error we double the step size and calculate the step again (done = false).
		/*else if(E_est <= getMinerror()) {
			step = 2.0*step;
			//done = false;
		}*/
//If the error estimate is larger than the specified maximum error we calculate a new step size and try to calculate the step again.
		else if(E_est >= getMaxerror()) {
			step = 0.9*step*pow(getMaxerror()/E_est,(1.0/3.0));
			//step = step*0.5;
		//	cout << "step size = " << step << endl;
			//If the new step size is smaller than the specified minimum step size we throw an exception and calculation stops.
			if(fabs(step) < getMinstep()) {
                            cout << "killed at: x = " << xx.back() << endl;
                                throw(cNumericalError("Minimum step size reached."));
                        }
			done = false;
		}
	}

	yy.push_back(yy.back() + step*(k1 + k2 + 4*k3)/6.0);
	dy.push_back(dy.back() + step*(l1 + l2 + 4*l3)/6.0);
	xx.push_back(xx.back() + step);
}
